// --------------------------------------------------------------------------------------------------------------------
// <copyright file="SLAM2DTest.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
namespace Microsoft.Robotics.Navigation.Runtime.UnitTests
{
    using System;
    using System.Collections.Generic;
    using System.Linq;

    using Microsoft.Robotics.Hardware.DAC;
    using Microsoft.Robotics.Navigation.Runtime.Localization;
    using Microsoft.Robotics.Numerics;
    using Microsoft.Robotics.Runtime;
    using Microsoft.Robotics.Tracing;
    using Microsoft.VisualStudio.TestTools.UnitTesting;

    using Test.Robotics.Metrics;
    using Test.Robotics.Navigation;
    using Test.Robotics.Runtime;    

    /// <summary>
    /// Basic unit test for SLAM2D
    /// </summary>
    [TestClass]
    public class SLAM2DTest
    {
        /// <summary>
        /// Run the SLAM pipeline
        /// </summary>
        [Priority(1)]
        [TestMethod]
        [TestCategory("Functional")]
        public void MiniSlamPipeline()
        {
            Manifest manifest = NavigationManifest.CreateSLAMPlaybackManifest(@"\\robofile\baker\Shared\Users\roboim\Recordings\SLAM\BLDG115\SmallLoop_TurretPan\EV4A-026", @"\\robofile\baker\Shared\Users\roboim\RobotConfig\EV4A-026", 1500, 1500, new Pose2D(200, 200, 0), 50);
            AgentLocator locator = new AgentLocator(true);

            IProducer<SLAM2DAgentMessage> slamAgent = (IProducer<SLAM2DAgentMessage>)manifest.Agents.Find(a => a.Name == "SLAM2DAgent");
            MessageCollectorAgent<SLAM2DAgentMessage> collector = new MessageCollectorAgent<SLAM2DAgentMessage>("collector", slamAgent, 1);
            manifest.Agents.Add(collector);
            manifest.ActiveAgents.Add(collector.Name);

            // start the manifest
            locator.LoadFrom(manifest);

            // Wait for the message to appear at end of pipeline
            collector.Wait(30000);

            locator.Deactivate();

            Assert.IsTrue(
                collector.Results.Count >= 1,
                string.Format("No SLAM messages were collected."));

            Assert.IsTrue(collector.Results.Dequeue().Poses[0].HasHeading);
        }

        /// <summary>
        /// Run the SLAM pipeline and verify the metrics results.
        /// </summary>
        [Priority(1)]
        [TestMethod]
        [TestCategory("Functional")]        
        [Timeout(60 * 60 * 1000)]
        [Ignore]
        public void SlamMetricsUnitTest()
        {
            Manifest manifest = NavigationManifest.CreateSlamMetricsUnitTest(@"\\robofile\baker\Shared\Users\roboim\Recordings\SLAM\BLDG115\SmallLoop_TurretPan_Unit", @"\\robofile\Baker\Shared\Users\roboim\Recordings\SLAM\BLDG115\SmallLoop\GroundTruth.txt", 1, new Pose2D(200, 200, 0), 800, 800, @"\\robofile\baker\Shared\Users\roboim\RobotConfig\EV4A-026", true, 100);            
            
            // find the metrics controller
            MetricsController resultGenerator;
            MessageCollectorAgent<CompletionMessage> completionAgent;

            MetricsTestRunner.AddMetricsResultsCompletionAgent(manifest, out resultGenerator, out completionAgent);

            List<MetricsResultMessage> resultCollection = new List<MetricsResultMessage>();

            // add an agent to aggregate test results
            DelegateAgent<MetricsResultMessage> resultAggegator = new DelegateAgent<MetricsResultMessage>(
                "resultAggegator",
                (IProducer<MetricsResultMessage>)resultGenerator,
                result => CollectTestResults(result, resultCollection));

            manifest.Agents.Add(resultAggegator);
            manifest.ActiveAgents.Add(resultAggegator.Name);

            MetricsTestRunner.RunManifest(manifest, TimeSpan.FromMinutes(50), true, completionAgent);

            List<MetricsResultMessage> positionResults = resultCollection.Where((result) => result.Type == "Position").ToList();
            List<MetricsResultMessage> orientationResults = resultCollection.Where((result) => result.Type == "Orientation").ToList();
            List<MetricsResultMessage> bkrPositionResults = resultCollection.Where((result) => result.Type == "BkrPosition").ToList();
            List<MetricsResultMessage> bkrOrientationResults = resultCollection.Where((result) => result.Type == "BkrOrientation").ToList();

            // BUG 1832 Investigate why we are dropping some ground truth points in slam test run
            ////Assert.IsTrue(positionResults.Count() == 10);
            ////Assert.IsTrue(bkrPositionResults.Count() == 9);

            Assert.IsTrue(positionResults.Average((result) => result.Accuracy) < 0.1);
            Assert.IsTrue(orientationResults.Average((result) => result.Accuracy) < 2);
            Assert.IsTrue(bkrPositionResults.Average((result) => result.Accuracy) < 0.2);
            Assert.IsTrue(bkrOrientationResults.Average((result) => result.Accuracy) < 2);    
        }

        /// <summary>
        /// Collects the test results.
        /// </summary>
        /// <param name="result">Metrics test result for one landmark point</param>
        /// <param name="resultCollection"> The result collection to which the results are added.</param>
        private static void CollectTestResults(MetricsResultMessage result, List<MetricsResultMessage> resultCollection)
        {
            TraceOut.Info(TraceContexts.Test, "Received result for ground truth id {0} Type {1}", result.GroundTruthId, result.Type);
            resultCollection.Add(result);
        }
    }
}
